#pragma once

#include "Robot.h"
#include "Track.h"
#include "SimResult.h"

class Environment {
	typedef boost::numeric::ublas::vector<double> vec;
	typedef boost::numeric::ublas::matrix<double> mat;

public:
	
	bool mDebug;
	double TimeStep;
	double EndTime;
	double Width;		// width and height of the "world" in cm
	double Height;

public:
	Environment(Robot *robot);
	~Environment();

	void RunSimulation();

	// get the current simulation time
	double T() const { return mTime; }

	Robot &GetRobot() { return *mRobot; }
	Track &GetTrack() { return mTrack; }
	const mat &GetCarMat() const { return mCarMat; }
	SimResult &GetResult() { return mResult; }

	int GetStep() const { return mStep; }

	const vec &GetCarPos() const { return mCarPos; }
	double GetCarDirection() const { return mDirection; }

private:
	// perform one step of the simulation
	void Step();

	// update the transformation matrix representing the car
	void UpdateCarMat();

	// save the current state of the simulation
	void Save();

private:
	SimResult mResult;
	Robot *mRobot;
	Track mTrack;
	
	vec mCarPos;	// position of the real axle of the car (2D)
	mat mCarMat;   // the car's transformation matrix (4x4)
	double mDirection;								// angle of car's longitudinal axis with respect to x-axis
	double mTime;									// current simulation time
	int mStep;										// the current step
};
